#include "stm32f4xx.h"
#include "pid.h"
#include "led.h"
#include "structconfig.h"
#include "control.h"
#include "imu.h"
#include "altitude.h"
/*****************************************************************************
*函  数：void PID_(PID_TYPE*PID,float target,float measure)
*功  能：位置式PID算法
*参  数：PID: 算法P I D参数的结构体
*        target: 目标值
*        measure: 测量值 
*返回值：无 
*备  注: 
*****************************************************************************/

////////////////////////////ROL角度外环控制////////////////////
void PID_Rol(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = PID->Error - PID->PreError;    //微分量
	//PID->Differ =	-Gyr_rad.X*RadtoDeg;    //微分量
	if(highaim>45)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	PID->Pout = (PID->P + 0.04f*fabs(PID->Error)) * PID->Error;     //比例控制:动态P
	//PID->Pout = PID->P * PID->Error;     //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
	
	PID->PreError = PID->Error ;                            //前一个误差值
}

////////////////////////////PIT角度外环控制////////////////////
void PID_Pit(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = PID->Error - PID->PreError;    //微分量
	//PID->Differ =	-Gyr_rad.Y*RadtoDeg;    //微分量
	if(highaim>45)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	PID->Pout = (PID->P + 0.04f*fabs(PID->Error)) * PID->Error;     //比例控制:动态P
	//PID->Pout = PID->P * PID->Error;     //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
	
	PID->PreError = PID->Error ;                            //前一个误差值
}

////////////////////////////偏航角控制////////////////////
void PID_Yaw(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	if(PID->Error>180) PID->Error=PID->Error-360;
	if(PID->Error<-180)PID->Error=PID->Error+360;
//	if(yaw>0&&PID->Error<-10) PID->Error=PID->Error+360;
//	if(yaw<0&&PID->Error>10) PID->Error=PID->Error-360;
	//PID->Differ = PID->Error - PID->PreError;    //微分量
	PID->Differ = -Gyr_rad.Z*RadtoDeg;    //微分量
	
	if(highaim>45)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	//PID->Pout = (PID->P + 0.03f*fabs(PID->Error)) * PID->Error;                        //比例控制
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
	
	PID->PreError = PID->Error ;                            //前一个误差值
}


////////////////////////////角速度内环控制////////////////////
void PID_Angle_R(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = PID->Error - PID->PreError;    //微分量
	
	if(highaim>45&&Uav_Enable)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
	
	PID->PreError = PID->Error ;                            //前一个误差值
}


////////////////////////////高度控制////////////////////
void PID_High(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = -V_Z;    //微分量
	
	if(highaim>45)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	//PID->Pout = (PID->P + 0.01f*fabs(PID->Error)) * PID->Error;     //比例控制:动态P
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
	
	PID->PreError = PID->Error ;                            //前一个误差值
}

////////////////////////////高度内环控制////////////////////
void PID_High_R(PID_TYPE*PID,float target,float measure)
{
//	int dt = 0;//采样时间(也就是扫描时间10ms)
	PID->Error  = target - measure;              //误差
	PID->Differ = PID->Error - PID->PreError;    //微分量
	
	if(Uav_Enable)    //飞机解锁之后再加入积分,防止积分过调
	{
		{
			PID->Integral += PID->Error;                        //对误差进行积分
			if(PID->Integral > PID->Irang)                      //积分限幅
          PID->Integral = PID->Irang;
		  if(PID->Integral < -PID->Irang)                     //积分限幅
			    PID->Integral = -PID->Irang;                    
		}
	}
	else
	{PID->Integral = 0;}
	
	PID->Pout = PID->P * PID->Error;                        //比例控制
	PID->Iout = PID->I * PID->Integral;  //积分控制
	PID->Dout = PID->D * PID->Differ;                       //微分控制
	
	PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;       //比例 + 积分 + 微分总控制
	
	PID->PreError = PID->Error ;                            //前一个误差值
}
